#include "sensor.h"
#include "mfl.h"  /* EASML Math Float Library */

/* Initialize sensor system */
void Sensor_Init(Sensor_System_Type *System_pst)
{
    uint8 i;
    
    /* Initialize all sensor structures */
    System_pst->Ultrasonic.Distance = 0.0f;
    System_pst->Ultrasonic.MaxRange = 4000.0f;  /* 4 meters max range */
    System_pst->Ultrasonic.IsValid = FALSE;
    
    System_pst->Infrared.Distance = 0.0f;
    System_pst->Infrared.Voltage = 0.0f;
    System_pst->Infrared.IsValid = FALSE;
    
    System_pst->Touch.IsPressed = FALSE;
    System_pst->Touch.LastState = FALSE;
    System_pst->Touch.PressTime = 0u;
    System_pst->Touch.ReleaseTime = 0u;
    
    System_pst->Color.Red = 0u;
    System_pst->Color.Green = 0u;
    System_pst->Color.Blue = 0u;
    System_pst->Color.Clear = 0u;
    System_pst->Color.Lux = 0.0f;
    System_pst->Color.IsValid = FALSE;
    
    System_pst->Sound.SoundLevel = 0.0f;
    System_pst->Sound.Threshold = 50.0f;  /* 50dB threshold */
    System_pst->Sound.IsTriggered = FALSE;
    
    System_pst->Gyro.GyroX = 0.0f;
    System_pst->Gyro.GyroY = 0.0f;
    System_pst->Gyro.GyroZ = 0.0f;
    System_pst->Gyro.AccelX = 0.0f;
    System_pst->Gyro.AccelY = 0.0f;
    System_pst->Gyro.AccelZ = 0.0f;
    System_pst->Gyro.Roll = 0.0f;
    System_pst->Gyro.Pitch = 0.0f;
    System_pst->Gyro.Yaw = 0.0f;
    System_pst->Gyro.IsValid = FALSE;
    
    /* Initialize EASML PT1 filters for sensor data */
    for (i = 0u; i < SENSOR_NUM; i++)
    {
        Sensor_FilterInit(&System_pst->FilterStates[i], SENSOR_FILTER_TC);
        System_pst->FilteredValues[i] = 0.0f;
    }
    
    System_pst->LastUpdateTime = 0u;
    System_pst->SystemReady = TRUE;
}

/* Initialize ultrasonic sensor */
void Sensor_UltrasonicInit(Sensor_Ultrasonic_Type *Sensor_pst, 
                          uint16 TrigPin_u16, uint16 EchoPin_u16, float32 MaxRange_f32)
{
    Sensor_pst->TrigPin = TrigPin_u16;
    Sensor_pst->EchoPin = EchoPin_u16;
    Sensor_pst->MaxRange = MaxRange_f32;
    Sensor_pst->Distance = 0.0f;
    Sensor_pst->LastMeasTime = 0u;
    Sensor_pst->IsValid = FALSE;
}

/* Read ultrasonic sensor */
void Sensor_UltrasonicRead(Sensor_Ultrasonic_Type *Sensor_pst)
{
    uint32 pulse_duration;
    float32 distance_raw;
    
    /* This would be implemented with hardware-specific code */
    /* For demonstration, we assume pulse_duration is obtained from hardware */
    pulse_duration = 1000u;  /* Example: 1000 microseconds */
    
    /* Calculate distance: distance = (pulse_duration * speed_of_sound) / 2 */
    /* Speed of sound = 343 m/s = 0.343 mm/μs */
    distance_raw = (float32)pulse_duration * 0.343f / 2.0f;
    
    /* Validate measurement */
    if (distance_raw > 0.0f && distance_raw <= Sensor_pst->MaxRange)
    {
        Sensor_pst->Distance = distance_raw;
        Sensor_pst->IsValid = TRUE;
    }
    else
    {
        Sensor_pst->IsValid = FALSE;
    }
}

/* Get ultrasonic distance */
float32 Sensor_UltrasonicGetDistance(const Sensor_Ultrasonic_Type *Sensor_cpst)
{
    return Sensor_cpst->IsValid ? Sensor_cpst->Distance : -1.0f;
}

/* Initialize infrared sensor */
void Sensor_InfraredInit(Sensor_Infrared_Type *Sensor_pst, uint16 AdcPin_u16)
{
    Sensor_pst->AdcPin = AdcPin_u16;
    Sensor_pst->RawValue = 0u;
    Sensor_pst->Distance = 0.0f;
    Sensor_pst->Voltage = 0.0f;
    Sensor_pst->IsValid = FALSE;
}

/* Read infrared sensor */
void Sensor_InfraredRead(Sensor_Infrared_Type *Sensor_pst, uint16 AdcValue_u16)
{
    float32 voltage, distance;
    
    Sensor_pst->RawValue = AdcValue_u16;
    
    /* Convert ADC value to voltage using EASML */
    voltage = Sensor_AdcToVoltage(AdcValue_u16, 4095u, 3.3f);  /* 12-bit ADC, 3.3V ref */
    Sensor_pst->Voltage = voltage;
    
    /* Convert voltage to distance (Sharp GP2Y0A21YK sensor characteristic) */
    if (voltage > 0.1f)
    {
        /* Using EASML power function: distance = 29.988 * voltage^(-1.173) */
        distance = 29.988f * Mfl_Pow_f32(voltage, -1.173f);
        
        /* Limit distance to reasonable range */
        distance = Mfl_Limit_f32(distance, 10.0f, 800.0f);  /* 1cm to 80cm */
        
        Sensor_pst->Distance = distance;
        Sensor_pst->IsValid = TRUE;
    }
    else
    {
        Sensor_pst->IsValid = FALSE;
    }
}

/* Get infrared distance */
float32 Sensor_InfraredGetDistance(const Sensor_Infrared_Type *Sensor_cpst)
{
    return Sensor_cpst->IsValid ? Sensor_cpst->Distance : -1.0f;
}

/* Initialize touch sensor */
void Sensor_TouchInit(Sensor_Touch_Type *Sensor_pst, uint16 Pin_u16)
{
    Sensor_pst->Pin = Pin_u16;
    Sensor_pst->IsPressed = FALSE;
    Sensor_pst->LastState = FALSE;
    Sensor_pst->PressTime = 0u;
    Sensor_pst->ReleaseTime = 0u;
}

/* Update touch sensor state */
void Sensor_TouchUpdate(Sensor_Touch_Type *Sensor_pst, boolean PinState_b, uint32 CurrentTime_u32)
{
    boolean current_state = !PinState_b;  /* Assuming active low */
    
    /* Detect state change */
    if (current_state != Sensor_pst->LastState)
    {
        if (current_state == TRUE)
        {
            /* Button pressed */
            Sensor_pst->PressTime = CurrentTime_u32;
            Sensor_pst->IsPressed = TRUE;
        }
        else
        {
            /* Button released */
            Sensor_pst->ReleaseTime = CurrentTime_u32;
            Sensor_pst->IsPressed = FALSE;
        }
        
        Sensor_pst->LastState = current_state;
    }
}

/* Check if touch sensor is pressed */
boolean Sensor_TouchIsPressed(const Sensor_Touch_Type *Sensor_cpst)
{
    return Sensor_cpst->IsPressed;
}

/* Check if touch sensor was just released */
boolean Sensor_TouchIsReleased(const Sensor_Touch_Type *Sensor_cpst)
{
    return (!Sensor_cpst->IsPressed && Sensor_cpst->LastState);
}

/* Initialize color sensor */
void Sensor_ColorInit(Sensor_Color_Type *Sensor_pst, uint8 I2cAddr_u8)
{
    Sensor_pst->I2cAddress = I2cAddr_u8;
    Sensor_pst->Red = 0u;
    Sensor_pst->Green = 0u;
    Sensor_pst->Blue = 0u;
    Sensor_pst->Clear = 0u;
    Sensor_pst->Lux = 0.0f;
    Sensor_pst->ColorTemp = 0u;
    Sensor_pst->IsValid = FALSE;
}

/* Read color sensor (TCS34725) */
void Sensor_ColorRead(Sensor_Color_Type *Sensor_pst)
{
    /* This would be implemented with I2C communication */
    /* For demonstration, we assume values are read from sensor */
    
    /* Example values */
    Sensor_pst->Red = 1500u;
    Sensor_pst->Green = 2000u;
    Sensor_pst->Blue = 1200u;
    Sensor_pst->Clear = 3000u;
    
    /* Calculate illuminance using EASML */
    if (Sensor_pst->Clear > 0u)
    {
        /* Lux calculation based on TCS34725 datasheet */
        float32 cpl = 2.4f;  /* Counts per lux */
        Sensor_pst->Lux = (float32)Sensor_pst->Clear / cpl;
        
        /* Calculate color temperature using approximation */
        float32 r_ratio = (float32)Sensor_pst->Red / (float32)Sensor_pst->Clear;
        float32 b_ratio = (float32)Sensor_pst->Blue / (float32)Sensor_pst->Clear;
        
        if (r_ratio != b_ratio)
        {
            float32 cct = 449.0f * Mfl_Pow_f32((r_ratio / b_ratio), -1.12f) + 3525.0f;
            Sensor_pst->ColorTemp = (uint16)Mfl_Round_f32(cct);
        }
        
        Sensor_pst->IsValid = TRUE;
    }
    else
    {
        Sensor_pst->IsValid = FALSE;
    }
}

/* Get RGB values from color sensor */
void Sensor_ColorGetRGB(const Sensor_Color_Type *Sensor_cpst, 
                       uint8 *Red_pu8, uint8 *Green_pu8, uint8 *Blue_pu8)
{
    if (Sensor_cpst->IsValid && Sensor_cpst->Clear > 0u)
    {
        /* Normalize to 8-bit values */
        *Red_pu8 = (uint8)Mfl_Min_f32(255.0f, 
                   (float32)Sensor_cpst->Red * 255.0f / (float32)Sensor_cpst->Clear);
        *Green_pu8 = (uint8)Mfl_Min_f32(255.0f, 
                     (float32)Sensor_cpst->Green * 255.0f / (float32)Sensor_cpst->Clear);
        *Blue_pu8 = (uint8)Mfl_Min_f32(255.0f, 
                    (float32)Sensor_cpst->Blue * 255.0f / (float32)Sensor_cpst->Clear);
    }
    else
    {
        *Red_pu8 = 0u;
        *Green_pu8 = 0u;
        *Blue_pu8 = 0u;
    }
}

/* Get illuminance from color sensor */
float32 Sensor_ColorGetLux(const Sensor_Color_Type *Sensor_cpst)
{
    return Sensor_cpst->IsValid ? Sensor_cpst->Lux : -1.0f;
}

/* Initialize filter using EASML PT1 filter */
void Sensor_FilterInit(Mfl_StatePT1_Type *FilterState_pst, float32 TimeConstant_f32)
{
    /* Initialize EASML PT1 filter state */
    Mfl_PT1SetState(FilterState_pst, 0.0f, 0.0f);
}

/* Update filter using EASML PT1 filter */
float32 Sensor_FilterUpdate(Mfl_StatePT1_Type *FilterState_pst, float32 Input_f32, float32 dT_f32)
{
    float32 k_filter = 1.0f;  /* Filter gain */
    float32 TeQ;
    
    /* Calculate equivalent time constant */
    TeQ = Mfl_CalcTeQ_f32(SENSOR_FILTER_TC, dT_f32);
    
    /* Update PT1 filter using EASML */
    Mfl_PT1Calc(Input_f32, FilterState_pst, k_filter, TeQ);
    
    return Mfl_PT1Out_f32(FilterState_pst);
}

/* Update entire sensor system */
void Sensor_SystemUpdate(Sensor_System_Type *System_pst, uint32 CurrentTime_u32)
{
    uint8 i;
    float32 dT_f32;
    
    /* Calculate time delta */
    dT_f32 = (float32)(CurrentTime_u32 - System_pst->LastUpdateTime) / 1000.0f;  /* Convert to seconds */
    
    if (dT_f32 > 0.001f)  /* Only update if enough time has passed */
    {
        /* Update filtered sensor values using EASML filters */
        System_pst->FilteredValues[0] = Sensor_FilterUpdate(&System_pst->FilterStates[0], 
                                                           System_pst->Ultrasonic.Distance, dT_f32);
        System_pst->FilteredValues[1] = Sensor_FilterUpdate(&System_pst->FilterStates[1], 
                                                           System_pst->Infrared.Distance, dT_f32);
        System_pst->FilteredValues[2] = Sensor_FilterUpdate(&System_pst->FilterStates[2], 
                                                           (float32)System_pst->Touch.IsPressed, dT_f32);
        System_pst->FilteredValues[3] = Sensor_FilterUpdate(&System_pst->FilterStates[3], 
                                                           System_pst->Color.Lux, dT_f32);
        System_pst->FilteredValues[4] = Sensor_FilterUpdate(&System_pst->FilterStates[4], 
                                                           System_pst->Sound.SoundLevel, dT_f32);
        System_pst->FilteredValues[5] = Sensor_FilterUpdate(&System_pst->FilterStates[5], 
                                                           System_pst->Gyro.Roll, dT_f32);
        
        System_pst->LastUpdateTime = CurrentTime_u32;
    }
}

/* Convert ADC value to voltage */
float32 Sensor_AdcToVoltage(uint16 AdcValue_u16, uint16 AdcMax_u16, float32 VRef_f32)
{
    return ((float32)AdcValue_u16 * VRef_f32) / (float32)AdcMax_u16;
}

/* Convert voltage to distance for IR sensor */
float32 Sensor_VoltageToDistance(float32 Voltage_f32)
{
    /* Sharp GP2Y0A21YK characteristic equation using EASML */
    if (Voltage_f32 > 0.1f)
    {
        return 29.988f * Mfl_Pow_f32(Voltage_f32, -1.173f);
    }
    else
    {
        return -1.0f;  /* Invalid reading */
    }
}
